#include "sc.h"
#include <math.h>

#define DEBUG_

NeighbourhoodToPixelOp()
gradx(in stream unsigned char i[-1..1][-3..3],
      out stream short int *o)
{
  int val = 0;
  signed char kernel[3][7];
  int y, x;

/*
      3     26     59      0    -59    -26     -3
      5     43     97      0    -97    -43     -5
      3     26     59      0    -59    -26     -3
*/

  kernel[0][0] = 3;
  kernel[0][1] = 26;
  kernel[0][2] = 59;
  kernel[0][3] = 0;
  kernel[0][4] = -59;
  kernel[0][5] = -26;
  kernel[0][6] = -3;

  kernel[1][0] = 5;
  kernel[1][1] = 43;
  kernel[1][2] = 97;
  kernel[1][3] = 0;
  kernel[1][4] = -97;
  kernel[1][5] = -43;
  kernel[1][6] = -5;

  kernel[2][0] = 3;
  kernel[2][1] = 26;
  kernel[2][2] = 59;
  kernel[2][3] = 0;
  kernel[2][4] = -59;
  kernel[2][5] = -26;
  kernel[2][6] = -3;

  for (y=-1; y <= 1; y++)
    for (x=-3; x <= 3; x++) 
      val += i[y][x] * (int)kernel[y+1][x+3];

  *o = val/1000;
}

NeighbourhoodToPixelOp()
grady(in stream unsigned char i[-3..3][-1..1],
      out stream short int *o)
{
  int val = 0;
  signed char kernel[7][3];
  int y, x;

  kernel[0][0] = 3;
  kernel[1][0] = 26;
  kernel[2][0] = 59;
  kernel[3][0] = 0;
  kernel[4][0] = -59;
  kernel[5][0] = -26;
  kernel[6][0] = -3;

  kernel[0][1] = 5;
  kernel[1][1] = 43;
  kernel[2][1] = 97;
  kernel[3][1] = 0;
  kernel[4][1] = -97;
  kernel[5][1] = -43;
  kernel[6][1] = -5;

  kernel[0][2] = 3;
  kernel[1][2] = 26;
  kernel[2][2] = 59;
  kernel[3][2] = 0;
  kernel[4][2] = -59;
  kernel[5][2] = -26;
  kernel[6][2] = -3;

  for (y=-3; y <= 3; y++)
    for (x=-1; x <= 1; x++) 
      val += i[y][x] * (int)kernel[y+3][x+1];

  *o = val/1000;
}

PixelToPixelOp()
gradmag(in stream short int *gx,
        in stream short int *gy,
        out stream unsigned char *o)
{
  short int ax = *gx, ay = *gy, m;

  if (ax < 0) ax = -ax;
  if (ay < 0) ay = -ay;

  m = ax+ay; 
  if (m > 255) m = 255;

  *o = m;
}

GlobalOp()
histthresh(in stream unsigned char **g,
           in double *p,
           out int *t)
{
  int hist[256], cum=0;
  int x, y, i;
  int target = *p*_stride*_lines;

  memset(hist, 0, 256*sizeof(int));

  for (y=0; y < _lines; y++)
    for (x=0; x < _stride; x++)
      hist[g[y][x]]++;

  for (i=255; i > 0 && cum < target; i--)
    cum += hist[i];

  *t = i;
}

PixelToPixelOp()
binarize(in stream unsigned char *i,
         in int *t,
         out stream unsigned char *o)
{
  *o = (*i > *t) * 255;
}

PixelToPixelOp()
orient(in stream unsigned char *i,
       in stream short int *gx,
       in stream short int *gy,
       out stream unsigned char *o)
{
  extern double atan2(double y, double x);
  extern double fabs(double x);

  if (*i)
  {
    double q = atan2(*gy, *gx);
    if (q < 0) q = 3.1415 + q;
    *o = q/3.1415*255;
  }
  else
    *o = 0;
}

NeighbourhoodToPixelOp()
nonmax(in stream unsigned char i[0][0],
       in stream unsigned char g[-1..1][-1..1],
       in stream unsigned char d[0][0],
       out stream unsigned char *o)
{
  extern double atan2(double y, double x);

  if (i[0][0])
  {
    float lg = g[0][0], ld = d[0][0]/255.0*3.1415;

    *o = 255;

    if (ld < atan2(0.5, 1.5))
    {
      if (g[ 0][-1] > lg || g[ 0][ 1] > lg)
        *o = 0;
    }
    else if (ld < atan2(1.5, 0.5))
    {
      if (g[-1][-1] > lg || g[ 1][ 1] > lg)
        *o = 0;
    }
    else if (ld < atan2(1.5, -0.5))
    {
      if (g[-1][ 0] > lg || g[ 1][ 0] > lg)
        *o = 0;
    }
    else if (ld < atan2(0.5, -1.5))
    {
      if (g[ 1][-1] > lg || g[-1][ 1] > lg)
        *o = 0;
    }
    else 
    {
      if (g[ 0][-1] > lg || g[ 0][ 1] > lg)
        *o = 0;
    }
  }
  else
    *o = 0;
}

NeighbourhoodToPixelOp()
detcorner(in stream unsigned char b[0][0],
          in stream short int gx[-1..1][-1..1],
          in stream short int gy[-1..1][-1..1],
          out stream unsigned char *o)
{
  if (b[0][0])
  {
    int y, x;
    float dxdx=0, dydy=0, dxdy=0, tr, det, res;
 

    for (y=-1; y <= 1; y++)
      for (x=-1; x <= 1; x++)
      {
        dxdx += gx[y][x]*gx[y][x];
        dydy += gy[y][x]*gy[y][x];
        dxdy += gx[y][x]*gy[y][x];
      }

    tr = dxdx + dydy;
    det = dxdx*dydy-dxdy*dxdy;

    res = (4*det/(tr*tr))*255;
    if (res < 0) res = -res;
    if (res > 255) res = 255;

    *o = res;
  }
  else
    *o = 0;
}

RecursiveNeighbourhoodToPixelOp()
thin(in stream unsigned char i[0..1][-1..1],
     out stream unsigned char o[-1..0][-1..1])
{
  unsigned char mask[256], nbh;

  init {
    int q;

    for (q = 0; q < 256; q++)
      mask[q] = 255;
  };

  if (i[0][0])
  {
    nbh = ((o[-1][-1]>0)<<0) | ((o[-1][ 0]>0)<<1) | ((o[-1][ 1]>0)<<2) |
          ((o[ 0][-1]>0)<<3) |                      ((i[ 0][ 1]>0)<<4) |
          ((i[ 1][-1]>0)<<5) | ((i[ 1][ 0]>0)<<6) | ((i[ 1][ 1]>0)<<7);

    if (mask[nbh])
      o[0][0] = 255;
    else
      o[0][0] = 0;
  }
  else
    o[0][0] = 0;
}

GlobalOp()
objdet(in stream unsigned char **i,
       in stream unsigned char **c)
{
  int ii;

  /* Just some waiting loop */
  for (ii=0; ii < 2048; ii++)
    c[0][0] = sqrt((float)c[0][0]*c[0][0]);
}

MAINTYPE MAIN(int argc, char **argv)
{
  Future<STREAM> img, gx, gy, g, b, o, c;
  Future<int> t;

  scInit(argc, argv);
  _allmulti(1);
  _setbuffersize(2097156);

#ifdef DEBUG
  capture(&img);
  write(&img, scint(0));
#else
  generate(scint(1024), &img);
#endif
  gradx(&img, &gx);
  grady(&img, &gy);
  _finalize(&img);
  gradmag(&gx, &gy, &g);
#ifdef DEBUG
  write(&g, scint(1));
#endif
  histthresh(&g, scdbl(0.1), &t);
  binarize(&g, &t, &b);
#ifdef DEBUG
  write(&b, scint(2));
#endif
  orient(&b, &gx, &gy, &o);
#ifdef DEBUG
  write(&o, scint(3));
#endif
  nonmax(&b, &g, &o, &b);
  _finalize(&g);
  _finalize(&o);
#ifdef DEBUG
  write(&b, scint(4));
#endif
  thin(&b, &b);
#ifdef DEBUG
  write(&b, scint(5));
#endif
  detcorner(&b, &gx, &gy, &c);
  _finalize(&gx);
  _finalize(&gy);
#ifdef DEBUG
  write(&c, scint(6));
#endif
  objdet(&b, &c);
  _finalize(&b);
  _finalize(&c);

  return scExit();
}
